load uv.mat
load cameraParams.mat
load planeParams_vertical

% 图像归一化坐标
inter = cameraParams.IntrinsicMatrix'; %相机内参数->归一化平面的坐标

%像素坐标系下坐标，(u,v)
u = uv(:,1); % 矩阵的第一列
v = uv(:,2); % 矩阵的第二列

%归一化 x_c/z_c = (u - u_0) / fx
norm_x = (u-inter(1,3))/inter(1,1);
%归一化 y_c/z_c = (v - v_0) / fy
norm_y = (v-inter(2,3))/inter(2,2);

% 加载激光方程,计算相机坐标系下的激光线
planeParams = planeParams_vertical;
pa = planeParams(1);
pb = planeParams(2);
pc = planeParams(3);
pd = planeParams(4);

%激光线的方程上的点（x_c, y_c, z_c），注意是数乘“点乘 .*”，“*”是矩阵乘法
z = -pd ./ (pa.*norm_x + pb.*norm_x + pc);
x = norm_x .* z;
y = norm_y .* z;

%% 变换到棋盘格平面世界坐标系并显示 激光线的三维坐标
RotationM = cameraParams.RotationMatrices;
TranslationV = cameraParams.TranslationVectors;
Rc_1 = RotationM(:,:,23)';
Tc_1 = TranslationV(23,:);

laser_x = x - Tc_1(1);
laser_y = y - Tc_1(2);
laser_z = z - Tc_1(3);

laser_camera = [laser_x laser_y laser_z];
laser_word = inv(Rc_1) * laser_camera';

xlimits = [floor(min(laser_word(1,:))),ceil(max(laser_word(1,:)))];
ylimits = [floor(min(laser_word(2,:))),ceil(max(laser_word(2,:)))];
zlimits = [floor(min(laser_word(3,:))),ceil(max(laser_word(3,:)))];

player3D = pcplayer(xlimits, ylimits, zlimits);
view(player3D,laser_word');